﻿using System;
    /// <summary>
    /// A Quaternion representing an orientation.
    /// </summary>
    [Serializable]
    public struct FixedQuaternion
    {

        /// <summary>The X component of the quaternion.</summary>
        public FixedPoint x;
        /// <summary>The Y component of the quaternion.</summary>
        public FixedPoint y;
        /// <summary>The Z component of the quaternion.</summary>
        public FixedPoint z;
        /// <summary>The W component of the quaternion.</summary>
        public FixedPoint w;

        public static readonly FixedQuaternion identity;

        static FixedQuaternion() {
            identity = new FixedQuaternion(0, 0, 0, 1);
        }

        /// <summary>
        /// Initializes a new instance of the JQuaternion structure.
        /// </summary>
        /// <param name="x">The X component of the quaternion.</param>
        /// <param name="y">The Y component of the quaternion.</param>
        /// <param name="z">The Z component of the quaternion.</param>
        /// <param name="w">The W component of the quaternion.</param>
        public FixedQuaternion(FixedPoint x, FixedPoint y, FixedPoint z, FixedPoint w)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

        public void Set(FixedPoint new_x, FixedPoint new_y, FixedPoint new_z, FixedPoint new_w) 
        {
            this.x = new_x;
            this.y = new_y;
            this.z = new_z;
            this.w = new_w;
        }

        public void SetFromToRotation(FixedVec3 fromDirection, FixedVec3 toDirection) 
        {
            FixedQuaternion targetRotation = FixedQuaternion.FromToRotation(fromDirection, toDirection);
            this.Set(targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w);
        }

        public FixedVec3 eulerAngles 
        {
            get {
                FixedVec3 result = new FixedVec3();

                FixedPoint ysqr = y * y;
                FixedPoint t0 = -FixedPoint.Two * (ysqr + z * z) + FixedPoint.One;//-2.0f * (ysqr + z * z) + 1.0f;
                FixedPoint t1 = +FixedPoint.Two * (x * y - w * z);//+2.0f * (x * y - w * z);
                FixedPoint t2 = -FixedPoint.Two * (x * z + w * y);//-2.0f * (x * z + w * y);
                FixedPoint t3 = +FixedPoint.Two * (y * z - w * x);//+2.0f * (y * z - w * x);
                FixedPoint t4 = -FixedPoint.Two * (x * x + ysqr) + FixedPoint.One;//-2.0f * (x * x + ysqr) + 1.0f;

                t2 = t2 > FixedPoint.One ? FixedPoint.One : t2;//t2 = t2 > 1.0f ? 1.0f : t2;
                t2 = t2 < FixedPoint.NegativeOne ? FixedPoint.NegativeOne : t2;//t2 = t2 < -1.0f ? -1.0f : t2;

                result.x = FixedMath.Radian2Angle(FixedMath.Atan2(t3, t4));//FixedMath.Atan2(t3, t4) * FixedPoint.Rad2Deg;
                result.y = FixedMath.Radian2Angle(FixedMath.Asin(t2));//FixedMath.Asin(t2) * FixedPoint.Rad2Deg;
                result.z = FixedMath.Radian2Angle(FixedMath.Atan2(t1, t0));//FixedMath.Atan2(t1, t0) * FixedPoint.Rad2Deg;

                return result * -1;
            }
        }

        public static FixedPoint Angle(FixedQuaternion a, FixedQuaternion b) {
            FixedQuaternion aInv = FixedQuaternion.Inverse(a);
            FixedQuaternion f = b * aInv;

            FixedPoint angle = FixedMath.Radian2Angle(FixedMath.Acos(f.w) * 2);//FixedMath.Acos(f.w) * 2 * FixedPoint.Rad2Deg;

            if (angle > 180) {
                angle = 360 - angle;
            }

            return angle;
        }

        /// <summary>
        /// Quaternions are added.
        /// </summary>
        /// <param name="quaternion1">The first quaternion.</param>
        /// <param name="quaternion2">The second quaternion.</param>
        /// <returns>The sum of both quaternions.</returns>
        #region public static JQuaternion Add(JQuaternion quaternion1, JQuaternion quaternion2)
        public static FixedQuaternion Add(FixedQuaternion quaternion1, FixedQuaternion quaternion2)
        {
            FixedQuaternion result;
            FixedQuaternion.Add(ref quaternion1, ref quaternion2, out result);
            return result;
        }

        public static FixedQuaternion LookRotation(FixedVec3 forward) {
            return CreateFromMatrix(FixedMatrix33.LookAt(forward, FixedVec3.up));
        }

        public static FixedQuaternion LookRotation(FixedVec3 forward, FixedVec3 upwards) {
            return CreateFromMatrix(FixedMatrix33.LookAt(forward, upwards));
        }

        public static FixedQuaternion Slerp(FixedQuaternion from, FixedQuaternion to, FixedPoint t) {
            t = FixedMath.Clamp(t, 0, 1);

            FixedPoint dot = Dot(from, to);

            if (dot < 0) {
                to = Multiply(to, -1);
                dot = -dot;
            }

            FixedPoint halfTheta = FixedMath.Acos(dot);

            return Multiply(Multiply(from, FixedMath.Sin((1 - t) * halfTheta)) + Multiply(to, FixedMath.Sin(t * halfTheta)), 1 / FixedMath.Sin(halfTheta));
        }

        public static FixedQuaternion RotateTowards(FixedQuaternion from, FixedQuaternion to, FixedPoint maxDegreesDelta) {
            FixedPoint dot = Dot(from, to);

            if (dot < 0) {
                to = Multiply(to, -1);
                dot = -dot;
            }

            FixedPoint halfTheta = FixedMath.Acos(dot);
            FixedPoint theta = halfTheta * 2;

            //maxDegreesDelta *= FixedPoint.Deg2Rad;
            maxDegreesDelta = FixedMath.Angle2Radian(maxDegreesDelta);

            if (maxDegreesDelta >= theta) {
                return to;
            }

            maxDegreesDelta /= theta;

            return Multiply(Multiply(from, FixedMath.Sin((1 - maxDegreesDelta) * halfTheta)) + Multiply(to, FixedMath.Sin(maxDegreesDelta * halfTheta)), 1 / FixedMath.Sin(halfTheta));
        }

        public static FixedQuaternion Euler(FixedPoint x, FixedPoint y, FixedPoint z)
        {
            x = FixedMath.Angle2Radian(x);//x *= FixedPoint.Deg2Rad;
            y = FixedMath.Angle2Radian(y);//y *= FixedPoint.Deg2Rad;
            z = FixedMath.Angle2Radian(z);//z *= FixedPoint.Deg2Rad;

            FixedQuaternion rotation;
            FixedQuaternion.CreateFromYawPitchRoll(y, x, z, out rotation);

            return rotation;
        }

        public static FixedQuaternion Euler(FixedVec3 eulerAngles) {
            return Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
        }

        public static FixedQuaternion AngleAxis(FixedPoint angle, FixedVec3 axis)
        {
            //axis = axis * FixedPoint.Deg2Rad;
            axis.x = FixedMath.Angle2Radian(axis.x);
            axis.x = FixedMath.Angle2Radian(axis.y);
            axis.x = FixedMath.Angle2Radian(axis.z);
            
            axis.Normalize();

            FixedPoint halfAngle = FixedMath.Angle2Radian(angle) * FixedPoint.Half;//angle * FixedPoint.Deg2Rad * FixedPoint.Half;

            FixedQuaternion rotation;
            FixedPoint sin = FixedMath.Sin(halfAngle);

            rotation.x = axis.x * sin;
            rotation.y = axis.y * sin;
            rotation.z = axis.z * sin;
            rotation.w = FixedMath.Cos(halfAngle);

            return rotation;
        }

        public static void CreateFromYawPitchRoll(FixedPoint yaw, FixedPoint pitch, FixedPoint roll, out FixedQuaternion result)
        {
            FixedPoint num9 = roll * FixedPoint.Half;
            FixedPoint num6 = FixedMath.Sin(num9);
            FixedPoint num5 = FixedMath.Cos(num9);
            FixedPoint num8 = pitch * FixedPoint.Half;
            FixedPoint num4 = FixedMath.Sin(num8);
            FixedPoint num3 = FixedMath.Cos(num8);
            FixedPoint num7 = yaw * FixedPoint.Half;
            FixedPoint num2 = FixedMath.Sin(num7);
            FixedPoint num = FixedMath.Cos(num7);
            result.x = ((num * num4) * num5) + ((num2 * num3) * num6);
            result.y = ((num2 * num3) * num5) - ((num * num4) * num6);
            result.z = ((num * num3) * num6) - ((num2 * num4) * num5);
            result.w = ((num * num3) * num5) + ((num2 * num4) * num6);
        }

        /// <summary>
        /// Quaternions are added.
        /// </summary>
        /// <param name="quaternion1">The first quaternion.</param>
        /// <param name="quaternion2">The second quaternion.</param>
        /// <param name="result">The sum of both quaternions.</param>
        public static void Add(ref FixedQuaternion quaternion1, ref FixedQuaternion quaternion2, out FixedQuaternion result)
        {
            result.x = quaternion1.x + quaternion2.x;
            result.y = quaternion1.y + quaternion2.y;
            result.z = quaternion1.z + quaternion2.z;
            result.w = quaternion1.w + quaternion2.w;
        }
        #endregion

        public static FixedQuaternion Conjugate(FixedQuaternion value)
        {
            FixedQuaternion quaternion;
            quaternion.x = -value.x;
            quaternion.y = -value.y;
            quaternion.z = -value.z;
            quaternion.w = value.w;
            return quaternion;
        }

        public static FixedPoint Dot(FixedQuaternion a, FixedQuaternion b) {
            return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
        }

        public static FixedQuaternion Inverse(FixedQuaternion rotation) {
            FixedPoint invNorm = FixedPoint.One / ((rotation.x * rotation.x) + (rotation.y * rotation.y) + (rotation.z * rotation.z) + (rotation.w * rotation.w));
            return FixedQuaternion.Multiply(FixedQuaternion.Conjugate(rotation), invNorm);
        }

        public static FixedQuaternion FromToRotation(FixedVec3 fromVector, FixedVec3 toVector) {
            FixedVec3 w = FixedVec3.Cross(fromVector, toVector);
            FixedQuaternion q = new FixedQuaternion(w.x, w.y, w.z, FixedVec3.Dot(fromVector, toVector));
            q.w += FixedMath.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);//FixedPoint.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
            q.Normalize();

            return q;
        }

        public static FixedQuaternion Lerp(FixedQuaternion a, FixedQuaternion b, FixedPoint t) {
            t = FixedMath.Clamp(t, FixedPoint.Zero, FixedPoint.One);

            return LerpUnclamped(a, b, t);
        }

        public static FixedQuaternion LerpUnclamped(FixedQuaternion a, FixedQuaternion b, FixedPoint t) {
            FixedQuaternion result = FixedQuaternion.Multiply(a, (1 - t)) + FixedQuaternion.Multiply(b, t);
            result.Normalize();

            return result;
        }

        /// <summary>
        /// Quaternions are subtracted.
        /// </summary>
        /// <param name="quaternion1">The first quaternion.</param>
        /// <param name="quaternion2">The second quaternion.</param>
        /// <returns>The difference of both quaternions.</returns>
        #region public static JQuaternion Subtract(JQuaternion quaternion1, JQuaternion quaternion2)
        public static FixedQuaternion Subtract(FixedQuaternion quaternion1, FixedQuaternion quaternion2)
        {
            FixedQuaternion result;
            FixedQuaternion.Subtract(ref quaternion1, ref quaternion2, out result);
            return result;
        }

        /// <summary>
        /// Quaternions are subtracted.
        /// </summary>
        /// <param name="quaternion1">The first quaternion.</param>
        /// <param name="quaternion2">The second quaternion.</param>
        /// <param name="result">The difference of both quaternions.</param>
        public static void Subtract(ref FixedQuaternion quaternion1, ref FixedQuaternion quaternion2, out FixedQuaternion result)
        {
            result.x = quaternion1.x - quaternion2.x;
            result.y = quaternion1.y - quaternion2.y;
            result.z = quaternion1.z - quaternion2.z;
            result.w = quaternion1.w - quaternion2.w;
        }
        #endregion

        /// <summary>
        /// Multiply two quaternions.
        /// </summary>
        /// <param name="quaternion1">The first quaternion.</param>
        /// <param name="quaternion2">The second quaternion.</param>
        /// <returns>The product of both quaternions.</returns>
        #region public static JQuaternion Multiply(JQuaternion quaternion1, JQuaternion quaternion2)
        public static FixedQuaternion Multiply(FixedQuaternion quaternion1, FixedQuaternion quaternion2)
        {
            FixedQuaternion result;
            FixedQuaternion.Multiply(ref quaternion1, ref quaternion2, out result);
            return result;
        }

        /// <summary>
        /// Multiply two quaternions.
        /// </summary>
        /// <param name="quaternion1">The first quaternion.</param>
        /// <param name="quaternion2">The second quaternion.</param>
        /// <param name="result">The product of both quaternions.</param>
        public static void Multiply(ref FixedQuaternion quaternion1, ref FixedQuaternion quaternion2, out FixedQuaternion result)
        {
            FixedPoint x = quaternion1.x;
            FixedPoint y = quaternion1.y;
            FixedPoint z = quaternion1.z;
            FixedPoint w = quaternion1.w;
            FixedPoint num4 = quaternion2.x;
            FixedPoint num3 = quaternion2.y;
            FixedPoint num2 = quaternion2.z;
            FixedPoint num = quaternion2.w;
            FixedPoint num12 = (y * num2) - (z * num3);
            FixedPoint num11 = (z * num4) - (x * num2);
            FixedPoint num10 = (x * num3) - (y * num4);
            FixedPoint num9 = ((x * num4) + (y * num3)) + (z * num2);
            result.x = ((x * num) + (num4 * w)) + num12;
            result.y = ((y * num) + (num3 * w)) + num11;
            result.z = ((z * num) + (num2 * w)) + num10;
            result.w = (w * num) - num9;
        }
        #endregion

        /// <summary>
        /// Scale a quaternion
        /// </summary>
        /// <param name="quaternion1">The quaternion to scale.</param>
        /// <param name="scaleFactor">Scale factor.</param>
        /// <returns>The scaled quaternion.</returns>
        #region public static JQuaternion Multiply(JQuaternion quaternion1, FixedPoint scaleFactor)
        public static FixedQuaternion Multiply(FixedQuaternion quaternion1, FixedPoint scaleFactor)
        {
            FixedQuaternion result;
            FixedQuaternion.Multiply(ref quaternion1, scaleFactor, out result);
            return result;
        }

        /// <summary>
        /// Scale a quaternion
        /// </summary>
        /// <param name="quaternion1">The quaternion to scale.</param>
        /// <param name="scaleFactor">Scale factor.</param>
        /// <param name="result">The scaled quaternion.</param>
        public static void Multiply(ref FixedQuaternion quaternion1, FixedPoint scaleFactor, out FixedQuaternion result)
        {
            result.x = quaternion1.x * scaleFactor;
            result.y = quaternion1.y * scaleFactor;
            result.z = quaternion1.z * scaleFactor;
            result.w = quaternion1.w * scaleFactor;
        }
        #endregion

        /// <summary>
        /// Sets the length of the quaternion to one.
        /// </summary>
        #region public void Normalize()
        public void Normalize()
        {
            FixedPoint num2 = (((this.x * this.x) + (this.y * this.y)) + (this.z * this.z)) + (this.w * this.w);
            FixedPoint num = 1 / (FixedMath.Sqrt(num2));
            this.x *= num;
            this.y *= num;
            this.z *= num;
            this.w *= num;
        }
        #endregion

        /// <summary>
        /// Creates a quaternion from a matrix.
        /// </summary>
        /// <param name="matrix">A matrix representing an orientation.</param>
        /// <returns>JQuaternion representing an orientation.</returns>
        #region public static JQuaternion CreateFromMatrix(JMatrix matrix)
        public static FixedQuaternion CreateFromMatrix(FixedMatrix33 matrix)
        {
            FixedQuaternion result;
            FixedQuaternion.CreateFromMatrix(ref matrix, out result);
            return result;
        }

        /// <summary>
        /// Creates a quaternion from a matrix.
        /// </summary>
        /// <param name="matrix">A matrix representing an orientation.</param>
        /// <param name="result">JQuaternion representing an orientation.</param>
        public static void CreateFromMatrix(ref FixedMatrix33 matrix, out FixedQuaternion result)
        {
            FixedPoint num8 = (matrix.M11 + matrix.M22) + matrix.M33;
            if (num8 > FixedPoint.Zero)
            {
                FixedPoint num = FixedMath.Sqrt((num8 + FixedPoint.One));
                result.w = num * FixedPoint.Half;
                num = FixedPoint.Half / num;
                result.x = (matrix.M23 - matrix.M32) * num;
                result.y = (matrix.M31 - matrix.M13) * num;
                result.z = (matrix.M12 - matrix.M21) * num;
            }
            else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
            {
                FixedPoint num7 = FixedMath.Sqrt((((FixedPoint.One + matrix.M11) - matrix.M22) - matrix.M33));
                FixedPoint num4 = FixedPoint.Half / num7;
                result.x = FixedPoint.Half * num7;
                result.y = (matrix.M12 + matrix.M21) * num4;
                result.z = (matrix.M13 + matrix.M31) * num4;
                result.w = (matrix.M23 - matrix.M32) * num4;
            }
            else if (matrix.M22 > matrix.M33)
            {
                FixedPoint num6 = FixedMath.Sqrt((((FixedPoint.One + matrix.M22) - matrix.M11) - matrix.M33));
                FixedPoint num3 = FixedPoint.Half / num6;
                result.x = (matrix.M21 + matrix.M12) * num3;
                result.y = FixedPoint.Half * num6;
                result.z = (matrix.M32 + matrix.M23) * num3;
                result.w = (matrix.M31 - matrix.M13) * num3;
            }
            else
            {
                FixedPoint num5 = FixedMath.Sqrt((((FixedPoint.One + matrix.M33) - matrix.M11) - matrix.M22));
                FixedPoint num2 = FixedPoint.Half / num5;
                result.x = (matrix.M31 + matrix.M13) * num2;
                result.y = (matrix.M32 + matrix.M23) * num2;
                result.z = FixedPoint.Half * num5;
                result.w = (matrix.M12 - matrix.M21) * num2;
            }
        }
        #endregion

        /// <summary>
        /// Multiply two quaternions.
        /// </summary>
        /// <param name="value1">The first quaternion.</param>
        /// <param name="value2">The second quaternion.</param>
        /// <returns>The product of both quaternions.</returns>
        #region public static FixedPoint operator *(JQuaternion value1, JQuaternion value2)
        public static FixedQuaternion operator *(FixedQuaternion value1, FixedQuaternion value2)
        {
            FixedQuaternion result;
            FixedQuaternion.Multiply(ref value1, ref value2,out result);
            return result;
        }
        #endregion

        /// <summary>
        /// Add two quaternions.
        /// </summary>
        /// <param name="value1">The first quaternion.</param>
        /// <param name="value2">The second quaternion.</param>
        /// <returns>The sum of both quaternions.</returns>
        #region public static FixedPoint operator +(JQuaternion value1, JQuaternion value2)
        public static FixedQuaternion operator +(FixedQuaternion value1, FixedQuaternion value2)
        {
            FixedQuaternion result;
            FixedQuaternion.Add(ref value1, ref value2, out result);
            return result;
        }
        #endregion

        /// <summary>
        /// Subtract two quaternions.
        /// </summary>
        /// <param name="value1">The first quaternion.</param>
        /// <param name="value2">The second quaternion.</param>
        /// <returns>The difference of both quaternions.</returns>
        #region public static FixedPoint operator -(JQuaternion value1, JQuaternion value2)
        public static FixedQuaternion operator -(FixedQuaternion value1, FixedQuaternion value2)
        {
            FixedQuaternion result;
            FixedQuaternion.Subtract(ref value1, ref value2, out result);
            return result;
        }
        #endregion

        /**
         *  @brief Rotates a {@link FixedVec3} by the {@link TSQuanternion}.
         **/
        public static FixedVec3 operator *(FixedQuaternion quat, FixedVec3 vec) 
        {
            FixedPoint num = quat.x * 2;
            FixedPoint num2 = quat.y * 2;
            FixedPoint num3 = quat.z * 2;
            FixedPoint num4 = quat.x * num;
            FixedPoint num5 = quat.y * num2;
            FixedPoint num6 = quat.z * num3;
            FixedPoint num7 = quat.x * num2;
            FixedPoint num8 = quat.x * num3;
            FixedPoint num9 = quat.y * num3;
            FixedPoint num10 = quat.w * num;
            FixedPoint num11 = quat.w * num2;
            FixedPoint num12 = quat.w * num3;

            FixedVec3 result;
            result.x = (1 - (num5 + num6)) * vec.x + (num7 - num12) * vec.y + (num8 + num11) * vec.z;
            result.y = (num7 + num12) * vec.x + (1 - (num4 + num6)) * vec.y + (num9 - num10) * vec.z;
            result.z = (num8 - num11) * vec.x + (num9 + num10) * vec.y + (1 - (num4 + num5)) * vec.z;

            return result;
        }

        public override string ToString() 
        {
            //return string.Format("({0:f1}, {1:f1}, {2:f1}, {3:f1})", x.AsFloat(), y.AsFloat(), z.AsFloat(), w.AsFloat());
            return $"{x}, {y}, {z}, {w}";
        }

    }